#coding:utf-8
# @ brief 读取txt或excel文件，并且进行图表可视化。
# @ data 2021/6/15
# @ author 许宇伟

import matplotlib.pyplot as plt 
import xlrd # excel read lib
mode = 'excel' # excel 或 txt
time_list_o = [] 
d_list_o = []
theta_list_o = []
rx_list_o = []
fp_list_o = []
if mode == 'txt':
    f = open('UWBdata.txt','r',encoding='utf-8')
    line = f.readline()

    while line:
        a = line.split() #按照空格划分
        time_list.append(float(a[0]))
        d_list.append(float(a[1]))
        theta_list.append(float(a[2]))
        rx_list.append(float(a[3]))
        fp_list.append(float(a[4]))
        line = f.readline()
    f.close()
elif mode == 'excel':
    book = xlrd.open_workbook('test.xlsx')
    sheet = book.sheet_by_index(0)
    time_list_o = sheet.col_values(0)
    d_list_o = sheet.col_values(6)
    theta_list_o = sheet.col_values(7)
    fp_list_o = sheet.col_values(8)
    rx_list_o = sheet.col_values(9)
    del time_list_o[0]
    del d_list_o[0]
    del theta_list_o[0]
    del fp_list_o[0]
    del rx_list_o[0]
    
time_zero = time_list_o[0]
subTime = lambda x:x-time_zero
time_new_o = list(map(subTime,time_list_o))

# cut
start = 0
end = 10000
time_new = time_new_o[start:end]
d_list = d_list_o[start:end]
theta_list = theta_list_o[start:end]
fp_list = fp_list_o[start:end]
rx_list = rx_list_o[start:end]


plt.figure(1)
plt.xlabel('time/ms')
plt.ylabel('Y')
ax1 = plt.subplot(2,2,1)
plt.plot(time_new,d_list,'o-',label="d")
plt.ylim(0.5,1.5)
plt.legend(loc='best')
ax1.set_title('dis')

ax2 = plt.subplot(222)
plt.plot(time_new,theta_list,'mo-',label="theta")
plt.ylim(-5,5)
plt.legend(loc='best')
ax2.set_title('theta')

ax3 = plt.subplot(212)
plt.ylim(-90,-45)
plt.plot(time_new,rx_list,'go-',label="rx",)
plt.plot(time_new,fp_list,'ro-',label="fp")
plt.legend(loc='best')
ax3.set_title('rx and fp')
plt.show()
# ax4 = plt.subplot(224)
# plt.ylim(-90,45)
# plt.plot(time_new,d_list,label="d")
# plt.plot(time_new,theta_list,label="theta")
# plt.plot(time_new,rx_list,label="rx")
# plt.plot(time_new,fp_list,label="fp")
# plt.legend(loc='best')
# ax4.set_title('overview')



